#ifndef __MOTOR_H__
#define __MOTOR_H__
#include "sys.h"

typedef enum { MOVE_BACK = -1, STOP = 0, MOVE_FORWARD = 1 } MotorDir;

typedef enum { MOTOR_LEFT = 0, MOTOR_RIGHT } MotorId;
typedef struct {
    MotorId id;
    MotorDir dir;
    s8 targetEncoder;
    s8 encoder;
    int allErr;
    int lastErr;
    int speed;
    int speedLimitLow;
    int speedLimitHigh;

    int (*GetEncoder)(MotorId);

    void (*SetTargetEncoder)(void *, int);
    void (*SetLimit)(void *, int, int);
    void (*SetMotorPwm)(void *);

    int (*CalcPIDResult)(void *, int, int);
    void (*LimitPwmDuty)(void *);
} Motor;

#define PWMA1 TIM4->CCR1
#define PWMA2 TIM4->CCR2

#define PWMB1 TIM4->CCR3
#define PWMB2 TIM4->CCR4
void PWM_Init(u16 arr, u16 psc);
void Motor_Gpio_init(void);
void motor_test(void);
void Set_Pwm(int motor_a, int motor_b);
void SetMotorPwm(Motor *motor);
void Set_Pwm_Motor_Right(int dir, int motor_a);
void Set_Pwm_Motor_Left(int dir, int motor_b);
void SetMotorSpeed(Motor *motor, int speed);
void SetMotorSpeedLimit(Motor *motor, int limitL, int limitH);

#endif
